#include "MMatrix4.hpp"


//! Constructor - sets identity matrix.
MMatrix4::MMatrix4()
{
	SetIdentity();
}

//! Constructor - argument is MMAtrix4 reference.
MMatrix4::MMatrix4( const MMatrix4& m )
{
	m_matrix[0]  = m.m_matrix[0];  
	m_matrix[4]  = m.m_matrix[4];  
	m_matrix[8]  = m.m_matrix[8];  
	m_matrix[12] = m.m_matrix[12]; 

	m_matrix[1]  = m.m_matrix[1];  
	m_matrix[5]  = m.m_matrix[5];  
	m_matrix[9]  = m.m_matrix[9];  
	m_matrix[13] = m.m_matrix[13]; 

	m_matrix[2]  = m.m_matrix[2];  
	m_matrix[6]  = m.m_matrix[6];  
	m_matrix[10] = m.m_matrix[10]; 
	m_matrix[14] = m.m_matrix[14]; 

	m_matrix[3]  = m.m_matrix[3];
	m_matrix[7]  = m.m_matrix[7];
	m_matrix[11] = m.m_matrix[11];
	m_matrix[15] = m.m_matrix[15];
}

//! Constructor - arguments are values for each matrix's cell.
MMatrix4::MMatrix4( float f0,  float f1,  float f2,  float f3,
					float f4,  float f5,  float f6,  float f7,
					float f8,  float f9,  float f10, float f11,
					float f12, float f13, float f14, float f15 )
{
    m_matrix[0]  = f0;  m_matrix[1]  = f1;  m_matrix[2]  = f2;  m_matrix[3]  = f3;
	m_matrix[4]  = f4;  m_matrix[5]  = f5;  m_matrix[6]  = f6;  m_matrix[7]  = f7;
	m_matrix[8]  = f8;  m_matrix[9]  = f9;  m_matrix[10] = f10; m_matrix[11] = f11;
	m_matrix[12] = f12; m_matrix[13] = f13; m_matrix[14] = f14; m_matrix[15] = f15;
}

//! Destructor.
MMatrix4::~MMatrix4()
{
}

//! Sets identity matrix.
void MMatrix4::SetIdentity()
{
	//! Set identity matrix
    m_matrix[0]  = 1.0f; m_matrix[1]  = 0.0f; m_matrix[2]  = 0.0f; m_matrix[3]  = 0.0f;
	m_matrix[4]  = 0.0f; m_matrix[5]  = 1.0f; m_matrix[6]  = 0.0f; m_matrix[7]  = 0.0f;
	m_matrix[8]  = 0.0f; m_matrix[9]  = 0.0f; m_matrix[10] = 1.0f; m_matrix[11] = 0.0f;
	m_matrix[12] = 0.0f; m_matrix[13] = 0.0f; m_matrix[14] = 0.0f; m_matrix[15] = 1.0f;
}

//! Sets all cells to zero.
void MMatrix4::SetZeros()
{
	for( int i = 0; i < 16; ++i )
		m_matrix[i] = 0;
}

//! Overloaded assignment operator.
MMatrix4& MMatrix4::operator =(const MMatrix4 &m)
{
	if( this != &m )
	{
		m_matrix[0]  = m.m_matrix[0];  
		m_matrix[4]  = m.m_matrix[4];  
		m_matrix[8]  = m.m_matrix[8];  
		m_matrix[12] = m.m_matrix[12]; 

		m_matrix[1]  = m.m_matrix[1];  
		m_matrix[5]  = m.m_matrix[5];  
		m_matrix[9]  = m.m_matrix[9];  
		m_matrix[13] = m.m_matrix[13]; 

		m_matrix[2]  = m.m_matrix[2];  
		m_matrix[6]  = m.m_matrix[6];  
		m_matrix[10] = m.m_matrix[10]; 
		m_matrix[14] = m.m_matrix[14]; 

		m_matrix[3]  = m.m_matrix[3];
		m_matrix[7]  = m.m_matrix[7];
		m_matrix[11] = m.m_matrix[11];
		m_matrix[15] = m.m_matrix[15];
	}

	return *this;
}

//! Overloaded product operator.
MMatrix4 MMatrix4::operator *(MMatrix4 &m)
{
	return MMatrix4( 
		//! Row: 1
		//! Element: matrix[0]
		m_matrix[0]*m.m_matrix[0] + m_matrix[1]*m.m_matrix[4] + m_matrix[2]*m.m_matrix[8] + m_matrix[3]*m.m_matrix[12],
		//! Element: matrix[1]
		m_matrix[0]*m.m_matrix[1] + m_matrix[1]*m.m_matrix[5] + m_matrix[2]*m.m_matrix[9] + m_matrix[3]*m.m_matrix[13],
		//! Element: matrix[2]
		m_matrix[0]*m.m_matrix[2] + m_matrix[1]*m.m_matrix[6] + m_matrix[2]*m.m_matrix[10] + m_matrix[3]*m.m_matrix[14],
		//! Element: matrix[3]
		m_matrix[0]*m.m_matrix[3] + m_matrix[1]*m.m_matrix[7] + m_matrix[2]*m.m_matrix[11] + m_matrix[3]*m.m_matrix[15],

		//! Row: 2
		//! Element: matrix[4]
		m_matrix[4]*m.m_matrix[0] + m_matrix[5]*m.m_matrix[4] + m_matrix[6]*m.m_matrix[8] + m_matrix[7]*m.m_matrix[12],
		//! Element: matrix[5]
		m_matrix[4]*m.m_matrix[1] + m_matrix[5]*m.m_matrix[5] + m_matrix[6]*m.m_matrix[9] + m_matrix[7]*m.m_matrix[13],
		//! Element: matrix[6]
		m_matrix[4]*m.m_matrix[2] + m_matrix[5]*m.m_matrix[6] + m_matrix[6]*m.m_matrix[10] + m_matrix[7]*m.m_matrix[14],
		//! Element: matrix[7]
		m_matrix[4]*m.m_matrix[3] + m_matrix[5]*m.m_matrix[7] + m_matrix[6]*m.m_matrix[11] + m_matrix[7]*m.m_matrix[15],

		//! Row: 3
		//! Element: matrix[8]
		m_matrix[8]*m.m_matrix[0] + m_matrix[9]*m.m_matrix[4] + m_matrix[10]*m.m_matrix[8] + m_matrix[11]*m.m_matrix[12],
		//! Element: matrix[9]
		m_matrix[8]*m.m_matrix[1] + m_matrix[9]*m.m_matrix[5] + m_matrix[10]*m.m_matrix[9] + m_matrix[11]*m.m_matrix[13],
		//! Element: matrix[10]
		m_matrix[8]*m.m_matrix[2] + m_matrix[9]*m.m_matrix[6] + m_matrix[10]*m.m_matrix[10] + m_matrix[11]*m.m_matrix[14],
		//! Element: matrix[11]
		m_matrix[8]*m.m_matrix[3] + m_matrix[9]*m.m_matrix[7] + m_matrix[10]*m.m_matrix[11] + m_matrix[11]*m.m_matrix[15],

		//! Row: 4
		//! Element: matrix[12]
		m_matrix[12]*m.m_matrix[0] + m_matrix[13]*m.m_matrix[4] + m_matrix[14]*m.m_matrix[8] + m_matrix[15]*m.m_matrix[12],
		//! Element: matrix[13]
		m_matrix[12]*m.m_matrix[1] + m_matrix[13]*m.m_matrix[5] + m_matrix[14]*m.m_matrix[9] + m_matrix[15]*m.m_matrix[13],
		//! Element: matrix[14]
		m_matrix[12]*m.m_matrix[2] + m_matrix[13]*m.m_matrix[6] + m_matrix[14]*m.m_matrix[10] + m_matrix[15]*m.m_matrix[14],
		//! Element: matrix[15]
		m_matrix[12]*m.m_matrix[3] + m_matrix[13]*m.m_matrix[7] + m_matrix[14]*m.m_matrix[11] + m_matrix[15]*m.m_matrix[15]
		);
}

//! Overloaded product operator.
MMatrix4 MMatrix4::operator *(float multiplier)
{
	return MMatrix4( m_matrix[0]*multiplier,  m_matrix[1]*multiplier,  m_matrix[2]*multiplier,  m_matrix[3]*multiplier,
					 m_matrix[4]*multiplier,  m_matrix[5]*multiplier,  m_matrix[6]*multiplier,  m_matrix[7]*multiplier, 
					 m_matrix[8]*multiplier,  m_matrix[9]*multiplier,  m_matrix[10]*multiplier, m_matrix[11]*multiplier,
					 m_matrix[12]*multiplier, m_matrix[13]*multiplier, m_matrix[14]*multiplier, m_matrix[15]*multiplier );
}

MVector3D MMatrix4::operator *(MVector3D& vec)
{
	MVector3D result;

	//first element
	result.x = m_matrix[0]*vec.x + m_matrix[1]*vec.y + m_matrix[2]*vec.z + m_matrix[3];

	//second element
	result.y = m_matrix[4]*vec.x + m_matrix[5]*vec.y + m_matrix[6]*vec.z + m_matrix[7];

	//third element
	result.z = m_matrix[8]*vec.x + m_matrix[9]*vec.y + m_matrix[10]*vec.z + m_matrix[11];

	return result;
}

//! Overloaded addition operator.
MMatrix4 MMatrix4::operator +(MMatrix4& m)
{
	return MMatrix4( m_matrix[0] + m.m_matrix[0], m_matrix[1] + m.m_matrix[1], m_matrix[2] + m.m_matrix[2],
		             m_matrix[3] + m.m_matrix[3], m_matrix[4] + m.m_matrix[4], m_matrix[5] + m.m_matrix[5],
					 m_matrix[6] + m.m_matrix[6], m_matrix[7] + m.m_matrix[7], m_matrix[8] + m.m_matrix[8],
					 m_matrix[9] + m.m_matrix[9], m_matrix[10] + m.m_matrix[10], m_matrix[11] + m.m_matrix[11],
					 m_matrix[12] + m.m_matrix[12], m_matrix[13] + m.m_matrix[13], m_matrix[14] + m.m_matrix[14],
					 m_matrix[15] + m.m_matrix[15] );
}

//! Overloaded subtract operator.
MMatrix4 MMatrix4::operator -(MMatrix4& m)
{
	return MMatrix4( m_matrix[0] - m.m_matrix[0], m_matrix[1] - m.m_matrix[1], m_matrix[2] - m.m_matrix[2],
		             m_matrix[3] - m.m_matrix[3], m_matrix[4] - m.m_matrix[4], m_matrix[5] - m.m_matrix[5],
					 m_matrix[6] - m.m_matrix[6], m_matrix[7] - m.m_matrix[7], m_matrix[8] - m.m_matrix[8],
					 m_matrix[9] - m.m_matrix[9], m_matrix[10] - m.m_matrix[10], m_matrix[11] - m.m_matrix[11],
					 m_matrix[12] - m.m_matrix[12], m_matrix[13] - m.m_matrix[13], m_matrix[14] - m.m_matrix[14],
					 m_matrix[15] - m.m_matrix[15] );
}

bool MMatrix4::InverseMatrix( MMatrix4 m )
{
// Copyright January 2004
// All Rights Reserved!
// Allen Sherrod
// ProgrammingAce@UltimateGameProgramming.com
// www.UltimateGameProgramming.com
//
// TEMPORARY BORROWED

   float tempMatrix[16] = {0};
   float d12, d13, d23, d24, d34, d41;

	d12 = m.m_matrix[2]  * m.m_matrix[7]  - m.m_matrix[3]  * m.m_matrix[6];
	d13 = m.m_matrix[2]  * m.m_matrix[11] - m.m_matrix[3]  * m.m_matrix[10];
	d23 = m.m_matrix[6]  * m.m_matrix[11] - m.m_matrix[7]  * m.m_matrix[10];
	d24 = m.m_matrix[6]  * m.m_matrix[15] - m.m_matrix[7]  * m.m_matrix[14];
	d34 = m.m_matrix[10] * m.m_matrix[15] - m.m_matrix[11] * m.m_matrix[14];
	d41 = m.m_matrix[14] * m.m_matrix[3]  - m.m_matrix[15] * m.m_matrix[2];

	tempMatrix[0] =   m.m_matrix[5] * d34 - m.m_matrix[9] * d24 + m.m_matrix[13] * d23;
	tempMatrix[1] = -(m.m_matrix[1] * d34 + m.m_matrix[9] * d41 + m.m_matrix[13] * d13);
	tempMatrix[2] =   m.m_matrix[1] * d24 + m.m_matrix[5] * d41 + m.m_matrix[13] * d12;
	tempMatrix[3] = -(m.m_matrix[1] * d23 - m.m_matrix[5] * d13 + m.m_matrix[9]  * d12);

	// Calculate the determinant.
	float determinant = m.m_matrix[0] * tempMatrix[0] + m.m_matrix[4] * tempMatrix[1] +
                       m.m_matrix[8] * tempMatrix[2] + m.m_matrix[12] * tempMatrix[3];

	// Clear if the determinant is equal to zero.  0 means matrix have no inverse.
	if(determinant == 0.0)
	   {
		   SetIdentity();
         return false;
	   }

	float invDeterminant = 1.0f / determinant;
	
	// Compute rest of inverse.
	tempMatrix[0] *= invDeterminant;
	tempMatrix[1] *= invDeterminant;
	tempMatrix[2] *= invDeterminant;
	tempMatrix[3] *= invDeterminant;

	tempMatrix[4] = -(m.m_matrix[4] * d34 - m.m_matrix[8] * d24 + m.m_matrix[12] * d23) * invDeterminant;
	tempMatrix[5] =   m.m_matrix[0] * d34 + m.m_matrix[8] * d41 + m.m_matrix[12] * d13  * invDeterminant;
	tempMatrix[6] = -(m.m_matrix[0] * d24 + m.m_matrix[4] * d41 + m.m_matrix[12] * d12) * invDeterminant;
	tempMatrix[7] =   m.m_matrix[0] * d23 - m.m_matrix[4] * d13 + m.m_matrix[8]  * d12  * invDeterminant;

	// Pre-compute 2x2 dets for first two rows when computing cofactors 
	// of last two rows.
	d12 = m.m_matrix[0]  * m.m_matrix[5]  - m.m_matrix[1]  * m.m_matrix[12];
	d13 = m.m_matrix[0]  * m.m_matrix[9]  - m.m_matrix[1]  * m.m_matrix[8];
	d23 = m.m_matrix[4]  * m.m_matrix[9]  - m.m_matrix[5]  * m.m_matrix[8];
	d24 = m.m_matrix[4]  * m.m_matrix[13] - m.m_matrix[5]  * m.m_matrix[12];
	d34 = m.m_matrix[8]  * m.m_matrix[13] - m.m_matrix[9]  * m.m_matrix[12];
	d41 = m.m_matrix[12] * m.m_matrix[1]  - m.m_matrix[13] * m.m_matrix[0];

	tempMatrix[8]  =   m.m_matrix[7] * d34 - m.m_matrix[11] * d24 + m.m_matrix[15] * d23 * invDeterminant;
	tempMatrix[9]  = -(m.m_matrix[3] * d34 + m.m_matrix[11] * d41 + m.m_matrix[15] * d13) * invDeterminant;
	tempMatrix[10] =   m.m_matrix[3] * d24 + m.m_matrix[7]  * d41 + m.m_matrix[15] * d12 * invDeterminant;
	tempMatrix[11] = -(m.m_matrix[3] * d23 - m.m_matrix[7]  * d13 + m.m_matrix[11] * d12) * invDeterminant;
	tempMatrix[12] = -(m.m_matrix[6] * d34 - m.m_matrix[10] * d24 + m.m_matrix[14] * d23) * invDeterminant;
	tempMatrix[13] =   m.m_matrix[2] * d34 + m.m_matrix[10] * d41 + m.m_matrix[14] * d13 * invDeterminant;
	tempMatrix[14] = -(m.m_matrix[2] * d24 + m.m_matrix[6]  * d41 + m.m_matrix[14] * d12) * invDeterminant;
	tempMatrix[15] =   m.m_matrix[2] * d23 - m.m_matrix[6]  * d13 + m.m_matrix[10] * d12 * invDeterminant;

   // Save the temp matrix to our matrix.
   m_matrix[0]  = tempMatrix[0];  m_matrix[1]  = tempMatrix[1];
   m_matrix[2]  = tempMatrix[2];  m_matrix[3]  = tempMatrix[3];
	m_matrix[4]  = tempMatrix[4];  m_matrix[5]  = tempMatrix[5];
   m_matrix[6]  = tempMatrix[6];  m_matrix[7]  = tempMatrix[7];
	m_matrix[8]  = tempMatrix[8];  m_matrix[9]  = tempMatrix[9];
   m_matrix[10] = tempMatrix[10]; m_matrix[11] = tempMatrix[11];
	m_matrix[12] = tempMatrix[12]; m_matrix[13] = tempMatrix[13];
   m_matrix[14] = tempMatrix[14]; m_matrix[15] = tempMatrix[15];

   return true;
}

void MMatrix4::TransposeMatrix()
{
	MMatrix4 tempMatrix = *this;

	//m_matrix[0] = tempMatrix.m_matrix[0];
	m_matrix[1] = tempMatrix.m_matrix[4];
	m_matrix[2] = tempMatrix.m_matrix[8];
	m_matrix[3] = tempMatrix.m_matrix[12];

	m_matrix[4] = tempMatrix.m_matrix[1];
	//m_matrix[5] = tempMatrix.m_matrix[5];
	m_matrix[6] = tempMatrix.m_matrix[9];
	m_matrix[7] = tempMatrix.m_matrix[13];

	m_matrix[8] = tempMatrix.m_matrix[2];
	m_matrix[9] = tempMatrix.m_matrix[6];
	//m_matrix[10] = tempMatrix.m_matrix[10];
	m_matrix[11] = tempMatrix.m_matrix[14];

	m_matrix[12] = tempMatrix.m_matrix[3];
	m_matrix[13] = tempMatrix.m_matrix[7];
	m_matrix[14] = tempMatrix.m_matrix[11];
	//m_matrix[15] = tempMatrix.m_matrix[15];
}